#include "motor_drivers.hpp"


void MotorDrivers::PID_Type::pidLog(){
    logPID.logWrite("Log PID::" + 
        std::to_string(thistime) + "\n"
        +"target" + std::to_string(target) + " \n"
        +"measure"+ std::to_string(measure) + "\n"
        +"err"    + std::to_string(err) +     "\n" 
        +"outs:"  + std::to_string(pout) + " " + std::to_string(iout) + " " + std::to_string(dout) + "\n"
        +"out:"   + std::to_string(output) +  "\n");
}

float MotorDrivers::PID_Type::pidCalc(float _measure)
{
    pidUpdatePeriod();
        measure = _measure;
        last_output = output;
        last_err = err;

        err = target - measure;

        if(std::abs(err) > DeadBand)
        {
            pout = kp * err;
            iout += ki * err;

            dout = kd * (err - last_err);

            if(iout > IntegralLimit) iout = IntegralLimit;
            else if(iout < -IntegralLimit) iout = -IntegralLimit;

            output = pout + iout + dout;
            //output = output*0.7f + last_output*0.3f  //滤波

            if(output > MaxOutput) output = MaxOutput;
            else if(output < -MaxOutput) output = -MaxOutput;

        }
        
        pidLog();
        return output;
}

void MotorDrivers::PID_Type::pidUpdatePeriod()
{
    lasttime = thistime;
    thistime = 
        static_cast<uint32_t> (std::chrono::duration_cast<std::chrono::milliseconds>
        (std::chrono::system_clock::now().time_since_epoch()).count());
    dtime = static_cast<uint8_t> (thistime - lasttime);
}
